/*
 * Copyright (c) 2019, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2019-08-26     sogwms       The first version
 */

#include "dual_pwm_motor.h"

struct dual_pwm_motor
{
    struct motor mot;
    rvm_dev_t *pwm1_dev;
    int pwm1_channel;
    rvm_dev_t *pwm2_dev;
    int pwm2_channel;
};

static unsigned long dual_pwm_motor_enable(void *mot)
{
    dual_pwm_motor_t mot_sub = (dual_pwm_motor_t)mot;

    rvm_hal_pwm_start(mot_sub->pwm1_dev, mot_sub->pwm1_channel);
    rvm_hal_pwm_start(mot_sub->pwm2_dev, mot_sub->pwm2_channel);    
    return 0;
}

static unsigned long dual_pwm_motor_disable(void *mot)
{
    dual_pwm_motor_t mot_sub = (dual_pwm_motor_t)mot;
    
    rvm_hal_pwm_stop(mot_sub->pwm1_dev, mot_sub->pwm1_channel);
    rvm_hal_pwm_stop(mot_sub->pwm2_dev, mot_sub->pwm2_channel);
    
    return 0;
}

static unsigned long dual_pwm_motor_reset(void *mot)
{
    return 0;
}

static unsigned long dual_pwm_motor_set_speed(void *mot, signed short percentage)
{
    dual_pwm_motor_t mot_sub = (dual_pwm_motor_t)mot;
    rvm_hal_pwm_config_t config;

    float temp_percentage = percentage *0.01;

    if (temp_percentage == 0)
    {
        config.duty_cycle = 0;
        config.freq       = MOTOR_PWM_PERIOD;
        config.polarity   = RVM_HAL_PWM_POLARITY_NORMAL;
        rvm_hal_pwm_config(mot_sub->pwm1_dev, &config, mot_sub->pwm1_channel);
        rvm_hal_pwm_config(mot_sub->pwm2_dev, &config, mot_sub->pwm2_channel);
    }
    else if (temp_percentage > 0)
    {
        config.duty_cycle = temp_percentage;
        config.freq       = MOTOR_PWM_PERIOD;
        config.polarity   = RVM_HAL_PWM_POLARITY_NORMAL;
        rvm_hal_pwm_config(mot_sub->pwm1_dev, &config, mot_sub->pwm1_channel);

        config.duty_cycle = 0;
        rvm_hal_pwm_config(mot_sub->pwm2_dev, &config, mot_sub->pwm2_channel);

    }
    else
    {
        config.duty_cycle = 0;
        config.freq       = MOTOR_PWM_PERIOD;
        config.polarity   = RVM_HAL_PWM_POLARITY_NORMAL;
        rvm_hal_pwm_config(mot_sub->pwm1_dev, &config, mot_sub->pwm1_channel);

        config.duty_cycle = -temp_percentage;
        rvm_hal_pwm_config(mot_sub->pwm2_dev, &config, mot_sub->pwm2_channel);
    }

    return 0;
}

dual_pwm_motor_t dual_pwm_motor_create(char *pwm1, int pwm1_channel, char *pwm2, int pwm2_channel)
{
    rvm_pwm_drv_register(0);

    dual_pwm_motor_t new_motor = (dual_pwm_motor_t)motor_create(sizeof(struct dual_pwm_motor));
    if (new_motor == NULL)
    {
        return NULL;
    }

    new_motor->pwm1_dev = (rvm_dev_t *)rvm_hal_pwm_open(pwm1); /* 查找PWM设备 */
    if (new_motor->pwm1_dev == NULL)
    {
        free(new_motor);
        aos_cli_printf("Falied to find device on %s", pwm1);
        return NULL;
    }
    new_motor->pwm2_dev = (rvm_dev_t *)rvm_hal_pwm_open(pwm1); /* 查找PWM设备 */
    if (new_motor->pwm2_dev == NULL)
    {
        free(new_motor);
        aos_cli_printf("Falied to find device on %s", pwm2);
        return NULL;
    }
    new_motor->pwm1_channel = pwm1_channel;
    new_motor->pwm2_channel = pwm2_channel;
    new_motor->mot.enable = dual_pwm_motor_enable;
    new_motor->mot.disable = dual_pwm_motor_disable;
    new_motor->mot.reset = dual_pwm_motor_reset;
    new_motor->mot.set_speed = dual_pwm_motor_set_speed;

    return new_motor;
}
